The present invention relates to apparatus for localization and tracking.
The theory of non-linear filtering and its applications are discussed in:
H. J. Kushner, xe2x80x9cApproximations to Optimal Nonlinear Filtersxe2x80x9d. IEEE Trans. A.C., Vol. AC-12, No. 5, October 1967;
A. Gelb, J. F. Kaspar, Jr., R. A. Nash, Jr., C. E. Price, and A. A. Southerland, Jr., xe2x80x9cApplied Optimal Estimationxe2x80x9d, M.I.T. Press, Cambridge, Mass., 1974;
B. D. O. Anderson, and J. B. Moore, xe2x80x9cOptimal Filteringxe2x80x9d, Prentice-Hall, Englewood Cliffs, N.J., 1979;
A. H. Jazwinski, xe2x80x9cStochastic Processes and Filtering Theoryxe2x80x9d, Academic Press, New York, 1971; and
M. S. Grewal, and A. P. Andrews, xe2x80x9cKalman Filteringxe2x80x9d, Prentice-Hall, Upper Saddle River, N.J., 1993.
The Biot-Savart law is discussed in:
J. D. Jackson, xe2x80x9cClassical Electrodynamicsxe2x80x9d, John Willey and Sons, New York, N.Y., 1975.
The application of Extended Kalman Filters (EKF) to tracking in the context of radar is discussed, for example, in U.S. Pat. Nos. 5,075,694, 4,179,696, 3,952,304 and 3,935,572. Other tracking systems are discussed, for example, in U.S. Pat. Nos. 5,095, 467 and 4,855,932.
The Kalman Filter is a standard tool for xe2x80x9cdata fusionxe2x80x9d of different sensors. In U.S. Pat. No. 5,416,712 GPS signals and dead reckoning are combined by a Kalman Filter, and where the gyro bias is also calibrated. In U.S. Pat. No. 5,645,077 automatic drift compensation is discussed.
The disclosures of all publications mentioned in the specification and of the publications cited therein are hereby incorporated by reference.
The present invention seeks to provide a non-linear Kalman Filter tracker.
There is thus provided in accordance with a preferred embodiment of the present invention pose tracking apparatus operative to track the pose of a moving object based on magnetic flux measurements taken in the vicinity of the moving object, the pose tracking apparatus including a non-linear Kalman filter-based tracker operative to receive magnetic flux measurements performed in the vicinity of the moving object, to operate a non-linear Kalman-type filter on the measurements, thereby to generate information regarding the pose of the moving object, and a pose indicator operative to provide an output indication of the information regarding the pose of the moving object.
Further in accordance with a preferred embodiment of the present invention the non-linear tracker includes an EKF (extended Kalman filter).
Additionally in accordance with a preferred embodiment of the present invention, the non-linear filter operates on a state vector whose components include pose coordinates and first time-derivatives of the pose coordinates.
Further in accordance with a preferred embodiment of the present invention the pose coordinates include 3 spatial coordinates and 2 orientation coordinates.
Further in accordance with a preferred embodiment of the present invention the apparatus also includes a transmitter array, which may include less than six operative transmitters, inducing magnetic flux in the vicinity of the moving object.
Still further in accordance with a preferred embodiment of the present invention the non-linear tracker employs a Biot-Savart transformation from the pose of the moving object to the magnetic flux measurements taken in its vicinity.
Still further in accordance with a preferred embodiment of the present invention the step of employing the Biot-Savart transformation includes computing a function h of a state vector "xgr", as follows:       h    ⁡          (      ξ      )        =                    C        0                    R        3              ⁢          (                                    3            ⁢                          A              1                        ⁢                          A              2                                            R            2                          -                  A          3                    )      
where C0 is a coefficient,
R is the distance between a detector detecting the magnetic flux measurements and a transmitter within the transmitter array; and
xe2x80x83A1=xcex4x sin(xcex8s)cos(xcfx86s)+xcex4y sin(xcex8s)sin(xcfx86s)+xcex4z cos(xcex8s)
A2=xcex4x sin(xcex8d)cos(xcfx86d)+xcex4y sin(xcex8d)sin(xcfx86d)+xcex4z cos(xcex8d)
A3=sin(xcex8s)cos(xcfx86s)sin(xcex8d)cos(xcfx86d)+sin(xcex8s)sin(xcfx86s)sin(xcex8d)sin(xcfx86d)+cos(xcfx86s)cos(xcfx86d)
and wherein the pose of the detector is (xd, yd, zd, xcex8d, xcfx86d) and the pose of the transmitter is (xs, ys, zs, xcex8s, xcfx86s), and where xcex4x, xcex4y and xcex4z denote the distance between the x, y and z components, respectively, of the detector""s pose and the transmitter""s pose.
Additionally in accordance with a preferred embodiment of the present invention the non-linear tracker approximates an elliptic integral, at least when the moving object is close to a transmitter within the transmitter array, by computing first and second terms of a Taylor series representing the elliptic integral.
Additionally in accordance with a preferred embodiment of the present invention, the approximated elliptic integral includes a correction to the above mentioned A1 and A3,
A1xe2x86x92A1(1xe2x88x92xcex4)
A3xe2x86x92A3(1xe2x88x92xcex7)
      δ    =                  5        8            ⁢                        (                      ρ            R                    )                2            ⁢              (                              7            ⁢                                          A                1                2                                            R                2                                              -          3                )                  η    =                  9        8            ⁢                        (                      ρ            R                    )                2            ⁢              (                              5            ⁢                                          A                1                2                                            R                2                                              -          1                )            
and where xcfx81 is the radius of the transmitter.
Still further in accordance with a preferred embodiment of the present invention the orientation component of the pose of the moving object is represented by two angles, continuous over time xcex8xe2x80x2 and xcfx86xe2x80x2, whose relationship with conventional polar coordinates xcex8 and xcfx86 is as follows:   θ  =      {                                                                      θ                xe2x80x2                                                                                                          if                                    ⁢                                      xe2x80x83                                    ⁢                                      mod                    (                                          θ                      ,                                              2                        ⁢                                                  xe2x80x83                                                ⁢                        π                                                              )                                                  ≤                π                                                                                        -                                  θ                  xe2x80x2                                                                                                                          if                                    ⁢                                      xe2x80x83                                    ⁢                                      mod                    (                                          θ                      ,                                              2                        ⁢                                                  xe2x80x83                                                ⁢                        π                                                              )                                                   greater than                 π                                                    ⁢                  
                ⁢        ϕ            =              {                                                            ϕ                xe2x80x2                                                                                                          if                                    ⁢                                      xe2x80x83                                    ⁢                  θ                                =                                  θ                  xe2x80x2                                                                                                                          ϕ                  xe2x80x2                                +                π                                                                                                          if                                    ⁢                                      xe2x80x83                                    ⁢                  θ                                =                                  -                                      θ                    xe2x80x2                                                                                          
Still further in accordance with a preferred embodiment of the present invention, in order to avoid singularity, a dynamic offset is described by the following transformation:
xe2x80x83xcex8=cosxe2x88x921[cos(xcex8xe2x80x2)cos(xcfx86xe2x80x2)]
xcfx86=cosxe2x88x921[{square root over (cos2(xcex8)+sin2(xcfx86xe2x80x2)cos2(xcex8xe2x80x2))}]
where xcex8 and xcfx86 include the orientation component of the moving object""s pose after the dynamic offset, and xcex8xe2x80x2 and xcfx86 include the orientation component of the moving object""s pose before the dynamic offset.
Still further in accordance with a preferred embodiment of the present invention the non-linear filter employs the following matrices and operations:
"xgr"k(xe2x88x92)="PHgr""xgr"kxe2x88x921(+)
where k is a time index, "xgr"k(xe2x88x92) is a state vector predictor, "xgr"k(+) is a state vector corrector, and "PHgr" is a state transition matrix,
Pk(xe2x88x92)="PHgr"Pkxe2x88x921(+)"khgr"T+Q
where P(xe2x88x92) is an estimate error covariance matrix predictor, P(+) is an estimate error covariance matrix corrector and Q is a process noise covariance matrix,                     "AutoLeftMatch"                              H            k                    =                                    ∂                              h                ⁢                                  (                                      ξ                    →                                    )                                                                    ∂                              ξ                →                                                        "RightBracketingBar"        "AutoRightMatch"              ξ      k        ⁢          (      -      )      
where h is a sensitivity function and "xgr" is a state vector,
Kk=Pk(xe2x88x92)HkT[HkPk(xe2x88x92)HkT+Rk]xe2x88x921
where Rk is a measurement noise covariance matrix,
"xgr"k(+)="xgr"k(xe2x88x92)+Kk{xcex6kxe2x88x92h["xgr"k(xe2x88x92)]}
where xcex6 denotes the magnetic flux measurements taken in the vicinity of the moving object, and
Pk(+)=[Ixe2x88x92KkHk]Pk(xe2x88x92)
Still further in accordance with a preferred embodiment of the present invention the magnetic flux measurements may include less than six magnetic flux measurements in the vicinity of the moving object.
Additionally in accordance with a preferred embodiment of the present invention, the non-linear tracker is operative to time-vary a measurement-noise covariance matrix R and a process-noise covariance matrix Q.
Further in accordance with a preferred embodiment of the present invention the time-varying R and Q includes:             R      k      dec        =                  R                  k          -          1                dec            ⁢              e                                            T              k                        -                          T                              k                -                1                                                          τ            decay                                          R      k        =                  R        k        dec            +              R        inf                        Q      k      dec        =                  Q                  k          -          1                dec            ⁢              e                                            T              k                        -                          T                              k                -                1                                                          τ            decay                                          Q      k        =                  Q        k        dec            +              Q        inf            
Still further in accordance with a preferred embodiment of the present invention at least one of the transmitters inducing a magnetic flux sampled by the measurements is self-calibrated.
Additionally in accordance with a preferred embodiment of the present invention, the non-linear filter is operative to calibrate the location of each of the self-calibrating transmitters.
Further in accordance with a preferred embodiment of the present invention the non-linear filter is operative to calibrate the intensity of each of the self-calibrating transmitters.
Still further in accordance with a preferred embodiment of the present invention the non-linear filter is operative to calibrate the radius of each of the self-calibrating transmitters.
Additionally in accordance with a preferred embodiment of the present invention, the non-linear filter is operative to calibrate the orientation of each of the self-calibrating transmitters.
Further in accordance with a preferred embodiment of the present invention, the tracker uses a state vector whose components comprise characteristics of the self-calibrated transmitters inducing a magnetic flux sampled by the measurements, and wherein at least one of the characteristics is self-calibrated.
Still further in accordance with a preferred embodiment of the present invention, the non-linear tracker employs a measurement noise matrix R and a process noise covariance matrix Q at least one of which is generated by an adaptive process.
Additionally in accordance with a preferred embodiment of the present invention the adaptive process for Q comprises
      Q    k    acc    =                    "AutoLeftMatch"                  α          ⁢                      (                                                                                ξ                    k                    velocity                                    ⁢                                      (                    +                    )                                                  -                                                      ξ                                          k                      -                      1                                        velocity                                    ⁢                                      (                    +                    )                                                                                                T                  k                                -                                  T                                      k                    -                    1                                                                        )                          "AutoRightMatch"            2        +                  (                  1          -          α                )            ⁢              Q                  k          -          1                acc            xe2x80x83Qk=Qkacc+Qoacc
"xgr"kvelocity(+) =estimate of {{dot over (x)}, {dot over (y)}, {dot over (z)}, {dot over (xcfx86)}, {dot over (xcex8)}}
Further in accordance with a preferred embodiment of the present invention the adaptive process for R comprises
Rks,s=xcex2{xcex6ksxe2x88x92hs["xgr"k(xe2x88x92)]}2+(1xe2x88x92xcex2)Rkxe2x88x921s,s
Rk=Rks,s+Ros,s